#pragma once
#include "ReconstructByKinectCommon.h"

class PatternCalibrationProcess
{
public:
	PatternCalibrationProcess (SensorDataInput *input);
	~PatternCalibrationProcess ();

	void initialize ();
	void clean ();

	void process ();
	glm::mat4x4 calculateExtrinsicMatrix ();
	float getScale () const;

private:
	const static int PATT_ROWS = 5;
	const static int PATT_COLUMNS = 7;
	const static int PATT_MIN = 5; // min (PATT_ROWS, PATT_COLUMNS)
	const static int PATT_Z_POINTS = PATT_MIN * (PATT_MIN - 1) / 2;

	SensorDataInput *_sensor;
	IplImage *_pattImage;
	IplImage *_pattImageShow;
	int _acc;
	float _scale;
	CvPoint2D32f _cornersAcc[PATT_ROWS * PATT_COLUMNS];
	glm::vec3 _cornersInWorldSpace[PATT_ROWS * PATT_COLUMNS + PATT_Z_POINTS];
	glm::vec3 _cornersInCamSpace[PATT_ROWS * PATT_COLUMNS + PATT_Z_POINTS];
	const CvMat *_pCameraMatrixConst;

	void _recognizePattern (IplImage *image);
	void _calculateCornersInCameraSpace ();
	glm::mat4 _calculateExtrinsicMatrix ();
	glm::mat4 _calculateTranslation ();
	glm::mat4 _calculateRotation ();
	glm::mat4 _calculateScale ();
	void _getCameraMatrix ();
	void _testResult (glm::mat4x4 rtsmat);
	glm::mat4 _calculateExtrinsicMatrixCv ();
	glm::mat4 _calculateExtrinsicMatrixBySolveEquation ();
};
